
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       crash_check.c
  * @author     baiyang
  * @date       2023-2-2
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
/*-----------------------------------macro------------------------------------*/
#define CRASH_CHECK_LOOP_RATE 200

// Code to detect a crash main ArduCopter code
#define CRASH_CHECK_TRIGGER_SEC         2       // 2 seconds inverted indicates a crash
#define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f   // 30 degrees beyond target angle is signal we are out of control
#define CRASH_CHECK_ANGLE_MIN_DEG       15.0f   // vehicle must be leaning at least 15deg to trigger crash check
#define CRASH_CHECK_SPEED_MAX           10.0f   // vehicle must be moving at less than 10m/s to trigger crash check
#define CRASH_CHECK_ACCEL_MAX           3.0f    // vehicle must be accelerating less than 3m/s/s to be considered crashed

// Code to detect a thrust loss main ArduCopter code
#define THRUST_LOSS_CHECK_TRIGGER_SEC         1     // 1 second descent while level and high throttle indicates thrust loss
#define THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD  1500  // we can't expect to maintain altitude beyond 15 degrees on all aircraft
#define THRUST_LOSS_CHECK_MINIMUM_THROTTLE    0.9f  // we can expect to maintain altitude above 90 % throttle

// Yaw imbalance check
#define YAW_IMBALANCE_IMAX_THRESHOLD 0.75f
#define YAW_IMBALANCE_WARN_MS 10000
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// crash_check - disarms motors if a crash has been detected
// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
// called at MAIN_LOOP_RATE, 200Hz
void fms_crash_check()
{
    static uint16_t crash_counter;  // number of iterations vehicle may have been crashed

    // return immediately if disarmed, or crash checking disabled
    if (!mb_motors_armed(fms.motors) || fms.ap.land_complete || fms.g.fs_crash_check == 0) {
        crash_counter = 0;
        return;
    }

    // exit immediately if in standby
    if (fms.standby_active) {
        crash_counter = 0;
        return;
    }

    // exit immediately if in force flying
    if (fms.force_flying && !mode_is_landing(fms.flightmode)) {
        crash_counter = 0;
        return;
    }

    // return immediately if we are not in an angle stabilize flight mode or we are flipping
    if (mode_number(fms.flightmode) == ACRO || mode_number(fms.flightmode) == FLIP) {
        crash_counter = 0;
        return;
    }

#if MODE_AUTOROTATE_ENABLED == ENABLED
    //return immediately if in autorotation mode
    if (mode_number(fms.flightmode) == AUTOROTATE) {
        crash_counter = 0;
        return;
    }
#endif

    // vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
    Vector3f_t land_accel_ef = lpf_get_output_vec3f(&fms.land_accel_ef_filter);
    const float filtered_acc = vec3_length(&land_accel_ef);
    if (filtered_acc >= CRASH_CHECK_ACCEL_MAX) {
        crash_counter = 0;
        return;
    }

    // check for lean angle over 15 degrees
    const float lean_angle_deg = degrees(acosf(fms.ahrs->cos_roll*fms.ahrs->cos_pitch));
    if (lean_angle_deg <= CRASH_CHECK_ANGLE_MIN_DEG) {
        crash_counter = 0;
        return;
    }

    // check for angle error over 30 degrees
    const float angle_error = attctrl_get_att_error_angle_deg(fms.attitude_control);
    if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
        crash_counter = 0;
        return;
    }

    // check for speed under 10m/s (if available)
    const Vector3f_t* vel_neu = ahrs_get_velocity_neu_cms(fms.ahrs);
    if (fms.ahrs->valid_lpos && (vec3_length(vel_neu) >= CRASH_CHECK_SPEED_MAX)) {
        crash_counter = 0;
        return;
    }

    // we may be crashing
    crash_counter++;

    // check if crashing for 2 seconds
    if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * CRASH_CHECK_LOOP_RATE)) {
        //AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH);
        // send message to gcs
        gcs_send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming: AngErr=%.0f>%.0f, Accel=%.1f<%.1f", angle_error, CRASH_CHECK_ANGLE_DEVIATION_DEG, filtered_acc, CRASH_CHECK_ACCEL_MAX);
        // disarm motors
        mb_arming_disarm(ARMING_METHOD_CRASH, true);
    }
}

// check for loss of thrust and trigger thrust boost in motors library
void fms_thrust_loss_check()
{
    static uint16_t thrust_loss_counter;  // number of iterations vehicle may have been crashed

    // no-op if suppresed by flight options param
    if ((fms.g.flight_options & (uint32_t)(DISABLE_THRUST_LOSS_CHECK)) != 0) {
        return;
    }

    // exit immediately if thrust boost is already engaged
    if (mb_motors_get_thrust_boost(fms.motors)) {
        return;
    }

    // return immediately if disarmed
    if (!mb_motors_armed(fms.motors) || fms.ap.land_complete) {
        thrust_loss_counter = 0;
        return;
    }

    // exit immediately if in standby
    if (fms.standby_active) {
        return;
    }

    // check for desired angle over 15 degrees
    // todo: add thrust angle to AC_AttitudeControl
    const Vector3f_t angle_target = attctrl_get_att_target_euler_cd(fms.attitude_control);
    if (sq(angle_target.x) + sq(angle_target.y) > sq(THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD)) {
        thrust_loss_counter = 0;
        return;
    }

    // check for throttle over 90% or throttle saturation
    if ((attctrl_get_throttle_in(fms.attitude_control) < THRUST_LOSS_CHECK_MINIMUM_THROTTLE) && (!fms.motors->limit.throttle_upper)) {
        thrust_loss_counter = 0;
        return;
    }

    // check throttle is over 25% to prevent checks triggering from thrust limitations caused by low commanded throttle
    if ((attctrl_get_throttle_in(fms.attitude_control) < 0.25f)) {
        thrust_loss_counter = 0;
        return;
    }

    // check for descent
    if (!math_flt_negative(ahrs_get_velocity_z_up_cms(fms.ahrs))) {
        thrust_loss_counter = 0;
        return;
    }

    // check for angle error over 30 degrees to ensure the aircraft has attitude control
    const float angle_error = attctrl_get_att_error_angle_deg(fms.attitude_control);
    if (angle_error >= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
        thrust_loss_counter = 0;
        return;
    }

    // the aircraft is descending with low requested roll and pitch, at full available throttle, with attitude control
    // we may have lost thrust
    thrust_loss_counter++;

    // check if thrust loss for 1 second
    if (thrust_loss_counter >= (THRUST_LOSS_CHECK_TRIGGER_SEC * CRASH_CHECK_LOOP_RATE)) {
        // reset counter
        thrust_loss_counter = 0;
        //AP::logger().Write_Error(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED);
        // send message to gcs
        gcs_send_text(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)mb_motors_get_lost_motor(fms.motors) + 1);
        // enable thrust loss handling
        mb_motors_set_thrust_boost(fms.motors, true);
        // the motors library disables this when it is no longer needed to achieve the commanded output
    }
}

// check for a large yaw imbalance, could be due to badly calibrated ESC or misaligned motors
void fms_yaw_imbalance_check()
{
    // no-op if suppresed by flight options param
    if ((fms.g.flight_options & (uint32_t)(DISABLE_YAW_IMBALANCE_WARNING)) != 0) {
        return;
    }

    // If I is disabled it is unlikely that the issue is not obvious
    if (!math_flt_positive(attctrl_get_rate_yaw_pid(fms.attitude_control)->_ki)) {
        return;
    }

    // thrust loss is triggered, yaw issues are expected
    if (mb_motors_get_thrust_boost(fms.motors)) {
        lpf_reset(&fms.yaw_I_filt, 0.0f);
        return;
    }

    // return immediately if disarmed
    if (!mb_motors_armed(fms.motors) || fms.ap.land_complete) {
        lpf_reset(&fms.yaw_I_filt, 0.0f);
        return;
    }

    // exit immediately if in standby
    if (fms.standby_active) {
        lpf_reset(&fms.yaw_I_filt, 0.0f);
        return;
    }

    // magnitude of low pass filtered I term
    const float I_term = attctrl_get_rate_yaw_pid(fms.attitude_control)->integrator;
    const float I = fabsf(lpf_apply2(&fms.yaw_I_filt, attctrl_get_rate_yaw_pid(fms.attitude_control)->integrator, 1.0f/CRASH_CHECK_LOOP_RATE));
    if (I > fabsf(I_term)) {
        // never allow to be larger than I
        lpf_reset(&fms.yaw_I_filt, I_term);
    }

    const float I_max = attctrl_get_rate_yaw_pid(fms.attitude_control)->_kimax;
    if ((math_flt_positive(I_max) && ((I > YAW_IMBALANCE_IMAX_THRESHOLD * I_max) || (math_flt_equal(I_term,I_max))))) {
        // filtered using over percentage of I max or unfiltered = I max
        // I makes up more than percentage of total available control power
        const uint32_t now = time_millis();
        if (now - fms.last_yaw_warn_ms > YAW_IMBALANCE_WARN_MS) {
            fms.last_yaw_warn_ms = now;
            gcs_send_text(MAV_SEVERITY_EMERGENCY, "Yaw Imbalance %0.0f%%", I *100);
        }
    }
}

/*------------------------------------test------------------------------------*/


